#include <stdio.h>
#include <unistd.h>
#include <termios.h>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <termio.h>

int scanKeyboard()
{
    int in;
    struct termios new_settings;
    struct termios stored_settings;
    tcgetattr(STDIN_FILENO,&stored_settings); //获得stdin 输入
    new_settings = stored_settings;           //
    new_settings.c_lflag &= (~ICANON);        //
    new_settings.c_cc[VTIME] = 0;
    tcgetattr(STDIN_FILENO,&stored_settings); //获得stdin 输入
    new_settings.c_cc[VMIN] = 1;
    tcsetattr(STDIN_FILENO,TCSANOW,&new_settings); //

    in = getchar();

    tcsetattr(STDIN_FILENO,TCSANOW,&stored_settings);
    return in;
}

int main(int argc, char *argv[])
{
        ros::init(argc, argv, "keyctrl");
        ros::NodeHandle keyctrl_node;
        ros::AsyncSpinner spinner(1); 
        spinner.start();
        
        ros::Publisher keypub = keyctrl_node.advertise<std_msgs::Int32>("arm_keyctrl", 100);

        std_msgs::Int32 _key_code;
        while (ros::ok())
        {
                _key_code.data = scanKeyboard();
                printf(":%d\r\n",_key_code.data);
                keypub.publish(_key_code);
                ros::spinOnce();          
        }
        ros::waitForShutdown();
        return 0;
}
